#include <btBulletCollisionCommon.h>
#include <IDroplet.h>
#include <IDropletSim.h>
#include <DropletColorTest/DropletColorTest.h>
#include <DropletDataStructs.h>
#include <DropletSimGlobals.h>

#include <cstdlib>
#include <cstdio>
#include <ctime>
#include <vector>
#include <utility>

#define DEFAULT_ROW_TILES 4
#define DEFAULT_COL_TILES 4
#define DEFAULT_NUM_DROPLETS 4
#define DEFAULT_TILE_LENGTH 24.0f
#define DEFAULT_DROPLET_RADIUS 2.0f

#define DEFAULT_TOTAL_TIMESTEPS 1000

IDropletSim sim;
int numRowTiles, numColTiles, numDroplets;
float tileLength, dropletRadius;
int totalTimesteps;
	
int btXWallShapeID, btYWallShapeID, btFloorShapeID, btDropletShapeID;

float getRandomf(float min, float max)
{
	float range = (max - min) + 1.0f;
	return min + float(range * rand() / (RAND_MAX + 1.0f));
}

void FillGlobals(int argc, char *argv[])
{
	numRowTiles		= DEFAULT_ROW_TILES;
	numColTiles		= DEFAULT_COL_TILES;
	numDroplets		= DEFAULT_NUM_DROPLETS;
	tileLength		= DEFAULT_TILE_LENGTH;
	dropletRadius	= DEFAULT_DROPLET_RADIUS;
	totalTimesteps	= DEFAULT_TOTAL_TIMESTEPS;
}

void InitSimulator()
{
	// Initialize Simulator
	SimSetupData setupData(
		numRowTiles, 
		numColTiles, 
		tileLength, 
		dropletRadius,
		true);
	sim.Init(setupData);

	// Set up simulator phyiscs objects
	btCollisionShape *floorShape = new btStaticPlaneShape(btVector3(0, 1, 0) , 0);
	btCollisionShape *xWallShape = new btBoxShape(btVector3(
		btScalar(tileLength * numRowTiles / 2), 
		btScalar(5.), 
		btScalar(1.)));
	btCollisionShape *yWallShape = new btBoxShape(btVector3(
		btScalar(1.), 
		btScalar(5.), 
		btScalar(tileLength * numColTiles / 2)));
	btCollisionShape *dropletShape = new btCylinderShape(btVector3(
		dropletRadius, 
		0.5, 
		dropletRadius));

	sim.AddCollisionShape(floorShape, &btFloorShapeID);
	sim.AddCollisionShape(xWallShape, &btXWallShapeID);
	sim.AddCollisionShape(yWallShape, &btYWallShapeID);
	sim.AddCollisionShape(dropletShape, &btDropletShapeID);

	// Create the floor and walls
	sim.CreateFloor(btFloorShapeID, btXWallShapeID, btYWallShapeID);
}

void SetupDroplets()
{
	float floorWidth = tileLength * numColTiles;
	float floorLength = tileLength * numRowTiles;
	float posRangeWidth = floorWidth / 2.0f;
	float posRangeLength = floorLength / 2.0f;

	for(int i = 0; i < DEFAULT_NUM_DROPLETS; i++)
	{
		// Set up the simulator/physics model
		ObjectPhysicsData *dropletPhyDat = (ObjectPhysicsData *)malloc(sizeof(ObjectPhysicsData));
		dropletPhyDat->colShapeIndex = btDropletShapeID;
		dropletPhyDat->mass = 1.0;
		dropletPhyDat->localInertia = btVector3(0.0, 0.0, 0.0);
		
		IDroplet *newDroplet = new DropletColorTest(dropletPhyDat);
		sim.AddDroplet(
			newDroplet, 
			std::make_pair(			
				getRandomf(-posRangeWidth + dropletRadius, posRangeWidth - dropletRadius),
				getRandomf(-posRangeLength + dropletRadius, posRangeLength - dropletRadius)),
			0.0f
		);
	}
}

void PrintColors(std::vector<unsigned char *> *colors)
{
	printf("======= COLORS ========\n");
	for(int i = 0; i < numDroplets; i++)
	{
		printf("[Droplet %i] Color (R, G , B) = (%i,\t%i,\t%i)\n", 
			i + 1, 
			(int)colors->at(i)[0],
			(int)colors->at(i)[1],
			(int)colors->at(i)[2]);
	}
}

void PrintPositions(std::vector<float *> *xyVals)
{
	printf("====== POSITIONS ======\n");
	for(int i = 0; i < numDroplets; i++)
	{
		printf("[Droplet %i] Postion (X, Y) = (%f,\t%f)\n",
			i + 1,
			xyVals->at(i)[0],
			xyVals->at(i)[1]);
	}
}

void main(int argc, char *argv[])
{
	FillGlobals(argc, argv);

	srand(time(0));
	InitSimulator();

	SetupDroplets();
	
	std::vector<unsigned char *> *dropletColors = new std::vector<unsigned char *>();
	std::vector<float *> *dropletPos = new std::vector<float *>();

	for(int i = 0; i < numDroplets; i++)
	{
		unsigned char *tmp1 = (unsigned char *)malloc(sizeof(unsigned char) * 3);
		dropletColors->push_back(tmp1);
		float *tmp2 = (float *)malloc(sizeof(float) * 2);
		dropletPos->push_back(tmp2);
	}

	for(int i = 0; i < DEFAULT_TOTAL_TIMESTEPS; i++)
	{
		printf("--------------- SIMULATION STEP %i ---------------\n" , i + 1);
		sim.Step();
		sim.GetDropletColors(dropletColors);
		PrintColors(dropletColors);

		sim.GetDropletPositions(dropletPos);
		PrintPositions(dropletPos);
		printf("\n");
	}

	sim.Cleanup();

	std::vector<unsigned char *>::reverse_iterator cit;
	for(cit = dropletColors->rbegin(); cit != dropletColors->rend(); cit++)
		free((unsigned char *)*cit);

	std::vector<float *>::reverse_iterator xyit;
	for(xyit = dropletPos->rbegin(); xyit != dropletPos->rend(); xyit++)
		free((float *)*xyit);

	delete dropletColors;
	delete dropletPos;
}